﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Research.Kinect.Nui;
using KinectUserManager;


namespace KinectXNATools
{
    public static class Texture2DHelper
    {
        public static Texture2D DepthToTexture2D(GraphicsDevice graphicsDevice, int[,] depthXY, int width, int height)
        {
            Texture2D tex = new Texture2D(graphicsDevice, width, height);
            Color[] colorsToDisplay = new Color[width * height];

            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    int col = depthXY[x, y] % 256;
                    Color color;
                    if (depthXY[x, y] == 0)
                    {
                        color = Color.Red;
                    }
                    else
                        color = new Color(col, col, col);

                    colorsToDisplay[y * width + x] = color;
                }
            }
            tex.SetData(colorsToDisplay);
            return tex;
        }
        public static Texture2D SkelletonImage(SkeletonData[] skell, Runtime KinectSkelletonRuntime, Rectangle Size, GraphicsDevice graphicsDevice, Color firstlineColor, Color firstjointColor, int firstjointScale, Color secondlineColor, Color secondjointColor, int secondjointScale, Color BackGround)
        {

            int max = Size.Width * Size.Height;
            Color[] skellColors = new Color[max];
            Texture2D skellTexure = new Texture2D(graphicsDevice, Size.Width, Size.Height);
            if (skell != null && skell.Length > 0)
            {
                skellColors = SkelletonImage(skell[0], skellColors, KinectSkelletonRuntime, Size, graphicsDevice, firstlineColor, firstjointColor, firstjointScale, BackGround);
            }

            if (skell != null && skell.Length > 1)
            {
                skellColors = SkelletonImage(skell[1], skellColors, KinectSkelletonRuntime, Size, graphicsDevice, secondlineColor, secondjointColor, secondjointScale, BackGround);
            }

            //rematar con negro lo que este transparente

            for (int i = 0; i < skellColors.Length; i++)
            {
                if (skellColors[i] != firstlineColor && skellColors[i] != firstjointColor && skellColors[i] != secondlineColor && skellColors[i] != secondjointColor)
                {
                    skellColors[i] = BackGround;
                }
            }
            skellTexure.SetData(skellColors);
            return skellTexure;
        }
        public static Color[] SkelletonImage(SkeletonData skell, Color[] skellColors, Runtime KinectSkelletonRuntime, Rectangle Size, GraphicsDevice graphicsDevice, Color lineColor, Color jointColor, int jointScale, Color BackGround)
        {

            Joint[] huesosBrazoR = new Joint[] { 
                skell.Joints[JointID.HandRight], skell.Joints[JointID.WristRight], skell.Joints[JointID.ElbowRight],
                    skell.Joints[JointID.ShoulderRight],skell.Joints[JointID.ShoulderCenter]};

            Joint[] huesosBrazoL = new Joint[] { 
                skell.Joints[JointID.HandLeft], skell.Joints[JointID.WristLeft], skell.Joints[JointID.ElbowLeft],
                    skell.Joints[JointID.ShoulderLeft],skell.Joints[JointID.ShoulderCenter]};

            Joint[] huesosPiernaR = new Joint[] { 
                skell.Joints[JointID.FootRight], skell.Joints[JointID.AnkleRight], skell.Joints[JointID.KneeRight],
                    skell.Joints[JointID.HipRight],skell.Joints[JointID.HipCenter]};

            Joint[] huesosPiernaL = new Joint[] { 
                skell.Joints[JointID.FootLeft], skell.Joints[JointID.AnkleLeft], skell.Joints[JointID.KneeLeft],
                    skell.Joints[JointID.HipLeft],skell.Joints[JointID.HipCenter]};

            Joint[] huesosCentro = new Joint[] { 
                skell.Joints[JointID.Head], skell.Joints[JointID.ShoulderCenter], skell.Joints[JointID.Spine],
                    skell.Joints[JointID.HipCenter]};
            //BrazoL
            Vector2[] LineasBrazoL = new Vector2[huesosBrazoL.Length];
            for (int i = 0; i < huesosBrazoL.Length; i++)
            {
                float depthX, depthY;
                KinectSkelletonRuntime.SkeletonEngine.SkeletonToDepthImage(huesosBrazoL[i].Position, out depthX, out depthY);
                LineasBrazoL[i] = new Vector2(Size.Width * depthX, Size.Height * depthY);
            }
            skellColors = Texture2DHelper.polyLineToColor(skellColors, LineasBrazoL, Size, lineColor);

            //BrazoR
            Vector2[] LineasBrazoR = new Vector2[huesosBrazoR.Length];
            for (int i = 0; i < huesosBrazoR.Length; i++)
            {
                float depthX, depthY;
                KinectSkelletonRuntime.SkeletonEngine.SkeletonToDepthImage(huesosBrazoR[i].Position, out depthX, out depthY);
                LineasBrazoR[i] = new Vector2(Size.Width * depthX, Size.Height * depthY);
            }
            skellColors = Texture2DHelper.polyLineToColor(skellColors, LineasBrazoR, Size, lineColor);

            //PiernaL
            Vector2[] LineasPiernaL = new Vector2[huesosPiernaL.Length];
            for (int i = 0; i < huesosPiernaL.Length; i++)
            {
                float depthX, depthY;
                KinectSkelletonRuntime.SkeletonEngine.SkeletonToDepthImage(huesosPiernaL[i].Position, out depthX, out depthY);
                LineasPiernaL[i] = new Vector2(Size.Width * depthX, Size.Height * depthY);
            }
            skellColors = Texture2DHelper.polyLineToColor(skellColors, LineasPiernaL, Size, lineColor);

            //PiernaR
            Vector2[] LineasPiernaR = new Vector2[huesosPiernaR.Length];
            for (int i = 0; i < huesosPiernaR.Length; i++)
            {
                float depthX, depthY;
                KinectSkelletonRuntime.SkeletonEngine.SkeletonToDepthImage(huesosPiernaR[i].Position, out depthX, out depthY);
                LineasPiernaR[i] = new Vector2(Size.Width * depthX, Size.Height * depthY);
            }
            skellColors = Texture2DHelper.polyLineToColor(skellColors, LineasPiernaR, Size, lineColor);

            //Center
            //PiernaL
            Vector2[] LineasCentro = new Vector2[huesosCentro.Length];
            for (int i = 0; i < huesosCentro.Length; i++)
            {
                float depthX, depthY;
                KinectSkelletonRuntime.SkeletonEngine.SkeletonToDepthImage(huesosCentro[i].Position, out depthX, out depthY);
                LineasCentro[i] = new Vector2(Size.Width * depthX, Size.Height * depthY);
            }
            skellColors = Texture2DHelper.polyLineToColor(skellColors, LineasCentro, Size, lineColor);

            skellColors = Texture2DHelper.paintJoints(KinectSkelletonRuntime, skell, skellColors, jointScale, Size, jointColor);

            return skellColors;
        }
        private static Color[] polyLineToColor(Color[] colors, Vector2[] dots, Rectangle Size, Color lineColor)
        {
            for (int i = 0; i < dots.Length - 1; i++)
            {

                // pendient ede trabajar
                Vector2 diference = dots[i + 1] - dots[i];
                diference = Vector2.Normalize(diference);
                float modulo = Vector2.Distance(dots[i + 1], dots[i]);

                for (int m = 0; m < modulo; m++)
                {
                    Vector2 punto = dots[i] + (diference * m);


                    int index = Size.Width * (int)(punto.Y) + (int)punto.X;
                    if (punto.Y < 0 || punto.Y > Size.Height)
                    {

                        index = (int)dots[i].X;
                    }

                    if (index > 0 && index < Size.Width * Size.Height)
                    {
                        colors[index] = lineColor;
                    }
                }

            }

            return colors;
        }
        private static Color[] paintJoints(Runtime KinectSkelletonRuntime, SkeletonData skell, Color[] colors, int jointScale, Rectangle Size, Color jointColor)
        {
            Vector2[] joints = new Vector2[skell.Joints.Count];
            //rellenar joints
            int n = 0;
            foreach (Joint j in skell.Joints)
            {
                //con dos huevos a ver si va, voy a modificar el array de joints on the fly
                KinectSkelletonRuntime.SkeletonEngine.SkeletonToDepthImage(j.Position, out joints[n].X, out joints[n].Y);
                joints[n].X *= Size.Width;
                joints[n].Y *= Size.Height;
                n++;
            }
            // por cada joint
            foreach (Vector2 v2 in joints)
            {
                //avanza en altura
                for (int y = -jointScale; y < jointScale; y++)
                {
                    //rellena en horizontal antes de avanzar en altura
                    for (int x = -jointScale; x < jointScale; x++)
                    {
                        int index = ((((int)v2.Y + y) * Size.Width) + ((int)v2.X + x));
                        if (index > 0 && index < colors.Length)
                        {
                            colors[index] = jointColor;
                        }
                    }
                }

            }

            return colors;
        }
        public static VectorXY getDisplayPosition(VectorXYZ joint, GraphicsDevice gDev, Runtime KinectSkelletonRuntime, Rectangle ImageSize)
        {

            float depthX, depthY;
            Vector pos = new Vector() { X = joint.X, Y = joint.Y, Z = joint.Z };
            KinectSkelletonRuntime.SkeletonEngine.SkeletonToDepthImage(pos, out depthX, out depthY);


            return new VectorXY() { X = (int)(depthX * ImageSize.Width), Y = (int)(depthY * ImageSize.Height) };

        }
        public static Texture2D TextureFromColor(GraphicsDevice graphicsDevice,Rectangle bounds, Color color)
        {
            Texture2D tex = new Texture2D(graphicsDevice, bounds.Width, bounds.Height);
            Color[]colorsForTex=new Color[bounds.Width*bounds.Height];
            for(int i=0;i<colorsForTex.Length;i++)
            {
                colorsForTex[i]=color;
            }
            tex.SetData(colorsForTex);
            return tex;
        }
    }
}
